//
// Created by liangwenhao on 2024/4/6.
//

#include "yaw_control.h"
Motor YawMotor;///can1 0x201
float YawMotor_Target_Angle = 0.765; // [0.745,0.785], InitAngle = 0.765
float YawEncoder_Real_Angle;
float YawMotor_CrossAngle;
float YawEncoder_Value;
uint8_t Yaw_Init_Flag = 0;


void YawMotor_Init()
{
    if(fabsf(YawEncoder_Real_Angle - YawMotor_Target_Angle) < 0.00005)
    {
        Yaw_Init_Flag = 1;
    }
}


void YawMotor_Control()
{
    if(fabsf(RC_Ctl.back_one.channel2)>0.8)
    {
        YawMotor_Target_Angle+=(float)RC_Ctl.back_one.channel2*0.000005;
        if(YawMotor_Target_Angle > 0.785) YawMotor_Target_Angle = 0.785;
        if(YawMotor_Target_Angle < 0.745) YawMotor_Target_Angle = 0.745;
    }
}